clear all
metersinlat=111043.07209801719;
metersinlon=84693.98967674395;
robotstat=getStatus(2);
origin=[robotstat.lat,robotstat.lon];
waypoint=[origin(1),origin(2)+10/metersinlon];

disp('send command1');
sendCommand(2, 'goToWaypoint',waypoint(1),waypoint(2),.5,1);
disp('command sent');
while(1)
    stat = getStatus(2)
    pause(0.5);
end